import time

class PCA9685:
    """
    PCA9685 16通道PWM控制器驱动类，适用于树莓派
    
    主要功能：
    - 初始化I2C连接
    - 设置PWM频率
    - 控制单个通道的PWM输出
    - 控制所有通道的PWM输出
    - 关闭单个或所有通道
    """
    
    # PCA9685寄存器定义
    MODE1 = 0x00        # 模式寄存器1
    MODE2 = 0x01        # 模式寄存器2
    SUBADR1 = 0x02      # I2C子地址1
    SUBADR2 = 0x03      # I2C子地址2
    SUBADR3 = 0x04      # I2C子地址3
    PRESCALE = 0xFE     # 预分频寄存器，用于设置PWM频率
    
    # LED0-LED15通道寄存器起始地址
    LED0_ON_L = 0x06    # LED0开时间低字节
    LED0_ON_H = 0x07    # LED0开时间高字节
    LED0_OFF_L = 0x08   # LED0关时间低字节
    LED0_OFF_H = 0x09   # LED0关时间高字节
    
    # 寄存器位定义
    RESTART = 0x80      # 重启位
    SLEEP = 0x10        # 睡眠位
    ALLCALL = 0x01      # 全部调用位
    INVRT = 0x10        # 反转输出位
    OUTDRV = 0x04       # 输出驱动模式位
    
    def __init__(self, address=0x40, bus_num=1):
        from smbus2 import SMBus
        """
        初始化PCA9685
        
        Args:
            address: PCA9685的I2C地址，默认0x40
            bus_num: I2C总线编号，树莓派默认1
        """
        self.address = address
        self.bus = SMBus(bus_num)
        self._reset()
    
    def _reset(self):
        """
        重置PCA9685芯片
        """
        # 设置MODE1寄存器，启用全部调用，正常模式
        self.bus.write_byte_data(self.address, self.MODE1, self.ALLCALL)
        # 设置MODE2寄存器，推挽输出，无反转，低电平有效
        self.bus.write_byte_data(self.address, self.MODE2, self.OUTDRV)
        time.sleep(0.005)  # 等待振荡器稳定
        
        # 读取MODE1寄存器值
        mode1 = self.bus.read_byte_data(self.address, self.MODE1)
        # 清除睡眠位
        mode1 &= ~self.SLEEP
        self.bus.write_byte_data(self.address, self.MODE1, mode1)
        time.sleep(0.005)  # 等待振荡器稳定
    
    def set_pwm_freq(self, freq_hz):
        """
        设置PWM输出频率
        
        Args:
            freq_hz: 期望的PWM频率，范围24Hz-1526Hz
        """
        # PCA9685的内部时钟为25MHz
        prescale_val = 25000000.0 / 4096.0 / freq_hz - 1.0
        prescale = int(round(prescale_val))
        
        # 进入睡眠模式以更新预分频器
        old_mode = self.bus.read_byte_data(self.address, self.MODE1)
        new_mode = (old_mode & ~self.RESTART) | self.SLEEP
        self.bus.write_byte_data(self.address, self.MODE1, new_mode)
        
        # 设置预分频值
        self.bus.write_byte_data(self.address, self.PRESCALE, prescale)
        
        # 恢复原始模式
        self.bus.write_byte_data(self.address, self.MODE1, old_mode)
        time.sleep(0.005)
        
        # 启用重启
        self.bus.write_byte_data(self.address, self.MODE1, old_mode | self.RESTART)
    
    def _set_pwm(self, channel, on, off):
        """
        设置单个通道的PWM值
        
        Args:
            channel: 通道号，0-15
            on: 开时间，0-4095
            off: 关时间，0-4095
        """
        if channel < 0 or channel > 15:
            raise ValueError("通道号必须在0-15之间")
        
        # 计算寄存器地址
        reg_on_l = self.LED0_ON_L + 4 * channel
        reg_on_h = self.LED0_ON_H + 4 * channel
        reg_off_l = self.LED0_OFF_L + 4 * channel
        reg_off_h = self.LED0_OFF_H + 4 * channel
        
        # 写入PWM值
        self.bus.write_byte_data(self.address, reg_on_l, on & 0xFF)
        self.bus.write_byte_data(self.address, reg_on_h, on >> 8)
        self.bus.write_byte_data(self.address, reg_off_l, off & 0xFF)
        self.bus.write_byte_data(self.address, reg_off_h, off >> 8)
    
    def set_pwm(self, channel, value):
        """
        设置单个通道的PWM占空比
        
        Args:
            channel: 通道号，0-15
            value: PWM值，0-4095（0=0%占空比，4095=100%占空比）
        """
        if value < 0 or value > 4095:
            raise ValueError("PWM值必须在0-4095之间")
        
        if value == 0:
            # 完全关闭
            self._set_pwm(channel, 0, 4096)
        elif value == 4095:
            # 完全开启
            self._set_pwm(channel, 4096, 0)
        else:
            # 正常PWM模式
            self._set_pwm(channel, 0, value)
    
    def set_all_pwm(self, value):
        """
        设置所有通道的PWM占空比
        
        Args:
            value: PWM值，0-4095（0=0%占空比，4095=100%占空比）
        """
        if value < 0 or value > 4095:
            raise ValueError("PWM值必须在0-4095之间")
        
        if value == 0:
            # 完全关闭所有通道
            self._set_pwm(16, 0, 4096)
        elif value == 4095:
            # 完全开启所有通道
            self._set_pwm(16, 4096, 0)
        else:
            # 正常PWM模式
            self._set_pwm(16, 0, value)
    
    def set_servo_angle(self, channel, angle):
        """
        设置舵机角度（适用于180度舵机）
        
        Args:
            channel: 通道号，0-15
            angle: 舵机角度，0-180度
        """
        if angle < 0 or angle > 180:
            raise ValueError("舵机角度必须在0-180度之间")
        
        # 舵机PWM值范围通常为500-2500us
        # 转换为0-4095的PWM值
        pulse_width = 500 + (angle / 180.0) * 2000
        # 假设频率为50Hz，周期为20ms
        pwm_value = int((pulse_width / 20000.0) * 4095)
        self.set_pwm(channel, pwm_value)
    
    def turn_off(self, channel):
        """
        关闭单个通道
        
        Args:
            channel: 通道号，0-15
        """
        self.set_pwm(channel, 0)
    
    def turn_off_all(self):
        """
        关闭所有通道
        """
        self.set_all_pwm(0)
    
    def turn_on(self, channel):
        """
        完全开启单个通道
        
        Args:
            channel: 通道号，0-15
        """
        self.set_pwm(channel, 4095)
    
    def turn_on_all(self):
        """
        完全开启所有通道
        """
        self.set_all_pwm(4095)
    
    def close(self):
        """
        关闭I2C连接
        """
        self.bus.close()

# 示例用法
if __name__ == "__main__":
    try:
        # 初始化PCA9685，使用默认地址0x40和总线1
        pca = PCA9685()
        
        # 设置PWM频率为50Hz（适合舵机）
        pca.set_pwm_freq(50)
        
        print("测试PCA9685驱动")
        print("按Ctrl+C退出")
        
        # 测试舵机控制
        while True:
            # 从0度到180度
            for angle in range(0, 181, 10):
                pca.set_servo_angle(0, angle)
                time.sleep(0.1)
            
            # 从180度到0度
            for angle in range(180, -1, -10):
                pca.set_servo_angle(0, angle)
                time.sleep(0.1)
                
    except KeyboardInterrupt:
        print("\n测试结束")
    finally:
        # 清理资源
        pca.turn_off_all()
        pca.close()